using BepuUtilities;
using System.Runtime.CompilerServices;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace BepuPhysics.Constraints
{
    /// <summary>
    /// 为具有Jacobian的约束提供共享功能,类似于BallSocket。
    /// </summary>
    public static class BallSocketShared
    {
        // TODO：这里可能会有一些额外的抽象级别的争论。如果我们给前一步函数提供它需要的数据(即姿态和惯性)
        // 直接地,跨不同约束共享实现会更容易。例如,使用一组特定的
        // 约束在一起-如球窝、摆动限制和旋转角度关节-您不会想要重新获得惯性并为所有这些约束摆姿势。
        // (在该特定情况下,为稳定起见,最好使用同时求解的球窝+旋转角关节,但该点保持不变。)
        // (还有..。任何时候使用组合约束时,都极有可能需要修改投影以优化内存打包。
        // 在极少数情况下,组合约束的自由度将小于3个.)
        // 不这么做的唯一原因是对编码基因的担忧。但我们可能不想仅仅因为JIT中一些希望不是永久性的怪癖而犹豫不决。
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ComputeEffectiveMass(ref BodyInertias inertiaA, ref BodyInertias inertiaB,
            ref Vector3Wide offsetA, ref Vector3Wide offsetB, ref Vector<float> effectiveMassCFMScale, out Symmetric3x3Wide effectiveMass)
        {
            // 附着到每个实体的锚点被约束为保持在相同位置,从而产生以下位置约束：
            // C=位置A+锚点偏移A-(位置B+锚点偏移B)=0
            // C‘=速度A+d/dt(锚点偏移A)-(速度B+d/dt(锚点偏移B))=0
            // C‘=速度A+角度速度x锚点偏移A-(速度B+角度速度B x锚点偏移B)=0
            // C‘=速度A*i+角度速度*skew对称(锚偏移A)-速度B*i-角速度B*斜对称(锚偏移B)=0
            // 所以,雅各布人：
            // 线性A：我
            // 角度A：skewSymmetric(AngorOffsetA)
            // 线人B：-I
            // 角度B：skewSymmetric(-anchorOffsetB)
            // 每个矩阵都是3x3矩阵。但是,我们不需要显式地计算或存储其中的任何内容。
            // 不存储单位矩阵是很明显的,但我们也可以只存储偏移量,而不是角雅可比的全部斜对称矩阵。
            // 用简单的叉积代替了斜对称矩阵。

            // 投影应努力包含约束工作所需的尽可能小的表示。
            // 我们可以某种程度上利用烘焙惯性到雅可比的把戏,因为我*软化的有效质量是…。软化有效质量。
            // 同样,J*inverseInertia只是两个线性贡献的标量逆均值。
            // 请注意,没有理由尝试将SoftenedEffectiveMasses烘焙到角雅可比中,因为我们已经至少需要执行一个
            // 由于线性组件,柔化的有效质量倍增。我们只需将原始的角雅可比和搭载在线性分量的有效质量乘积上即可。

            // 所以,问题就变成了.有没有办法把逆Inertia捆绑到有角度的雅可比中？
            // 是的,但是没有用。如果我们将CSIToWSV的Skew(Offset)*InverseInertia预乘,结果不再是对称的。
            // 这意味着,与对称的逆惯性相比,每个物体需要多存储3个标量。这并不是特别有用;
            // 它只节省了一个叉积。加载更多6个标量来节省2个叉积(12个乘法,6个加法)是一种可怕的交易,即使在SIMD128上也是如此。

            Symmetric3x3Wide.SkewSandwichWithoutOverlap(offsetA, inertiaA.InverseInertiaTensor, out var inverseEffectiveMass);
            // 请注意,雅可比在技术上是偏对称的(-OffsetB),但是由于三明治的缘故,符号无关紧要。
            Symmetric3x3Wide.SkewSandwichWithoutOverlap(offsetB, inertiaB.InverseInertiaTensor, out var angularBContribution);
            Symmetric3x3Wide.Add(inverseEffectiveMass, angularBContribution, out inverseEffectiveMass);

            // 线性贡献简单地说就是i*逆质量*i,它只是增加了对角线。
            var linearContribution = inertiaA.InverseMass + inertiaB.InverseMass;
            inverseEffectiveMass.XX += linearContribution;
            inverseEffectiveMass.YY += linearContribution;
            inverseEffectiveMass.ZZ += linearContribution;
            Symmetric3x3Wide.Invert(inverseEffectiveMass, out effectiveMass);
            Symmetric3x3Wide.Scale(effectiveMass, effectiveMassCFMScale, out effectiveMass);


            // 注：
            // 可以使用对角的局部惯性张量,每个物体只有3个标量。假设我们加载方向作为前一步的一部分,因此可以重建
            // 世界惯性张量,这将节省6个标量载荷。单独来看,这很可能是一场胜利。但是,还有其他一些考虑因素：
            // 1)一些常见约束(特别是包括接触约束)不加载姿势。它们可以加载带有方向的对角化惯性(每个身体总共7个标量)。
            // 或对称的世界惯性张量(6个标量)。考虑到非相干道集的成本相对较高,将使用6标量世界惯性道集。
            // 2)如果同时从局部惯性和世界惯性加载约束,而不是从其中之一加载约束,则缓存未命中的数量会增加。
            // 3)要求形状的局部空间与转动惯量对齐。虽然这可以在创建形状时自动完成,并且
            // 所有基本体形状都是用对角惯性张量构建的,很可能对角化所需的局部"重定向"适用于
            // 凸起的外壳或网格会让用户感到困惑。重新定位当然是在V1中。
            // 4)它不能用于节省约束中显式存储的惯性的空间,因为热启动/求解不加载姿势(如上所述,加载方向和
            // 局部惯量比世界惯量大1个标量)。
            // 5)虽然局部对角表示在前一步中确实为ALU级优化提供了一些可能性,但有效质量矩阵将在世界空间中,
            // 一般来说,它不会比通常的对称表示法小。
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocities velocityB,
            ref Vector3Wide offsetA, ref Vector3Wide offsetB, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector3Wide constraintSpaceImpulse)
        {
            Vector3Wide.CrossWithoutOverlap(offsetA, constraintSpaceImpulse, out var wsi);
            Symmetric3x3Wide.TransformWithoutOverlap(wsi, inertiaA.InverseInertiaTensor, out var change);
            Vector3Wide.Add(velocityA.Angular, change, out velocityA.Angular);

            Vector3Wide.Scale(constraintSpaceImpulse, inertiaA.InverseMass, out change);
            Vector3Wide.Add(velocityA.Linear, change, out velocityA.Linear);

            Vector3Wide.CrossWithoutOverlap(constraintSpaceImpulse, offsetB, out wsi); // 音符翻转否定
            Symmetric3x3Wide.TransformWithoutOverlap(wsi, inertiaB.InverseInertiaTensor, out change);
            Vector3Wide.Add(velocityB.Angular, change, out velocityB.Angular);

            Vector3Wide.Scale(constraintSpaceImpulse, inertiaB.InverseMass, out change);
            Vector3Wide.Subtract(velocityB.Linear, change, out velocityB.Linear); // 音符减法;雅可比是-i
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ComputeCorrectiveImpulse(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref Vector3Wide offsetA, ref Vector3Wide offsetB,
            ref Vector3Wide biasVelocity, ref Symmetric3x3Wide effectiveMass, ref Vector<float> softnessImpulseScale, ref Vector3Wide accumulatedImpulse, out Vector3Wide correctiveImpulse)
        {
            // CSI=投射.BiasImPulse-累加脉冲*投射.SoftnessImPulseScale-(csia线性+csiaAngular+csib线性+csibAngular);
            // 音符减法;jLinearB=-I。
            Vector3Wide.Subtract(velocityA.Linear, velocityB.Linear, out var csv);
            Vector3Wide.CrossWithoutOverlap(velocityA.Angular, offsetA, out var angularCSV);
            Vector3Wide.Add(csv, angularCSV, out csv);
            // 注意交叉顺序颠倒;匹配雅可比交叉矩阵(OffsetB)。
            Vector3Wide.CrossWithoutOverlap(offsetB, velocityB.Angular, out angularCSV);
            Vector3Wide.Add(csv, angularCSV, out csv);
            Vector3Wide.Subtract(biasVelocity, csv, out csv);

            Symmetric3x3Wide.TransformWithoutOverlap(csv, effectiveMass, out correctiveImpulse);
            Vector3Wide.Scale(accumulatedImpulse, softnessImpulseScale, out var softness);
            Vector3Wide.Subtract(correctiveImpulse, softness, out correctiveImpulse);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref Vector3Wide offsetA, ref Vector3Wide offsetB,
            ref Vector3Wide biasVelocity, ref Symmetric3x3Wide effectiveMass, ref Vector<float> softnessImpulseScale, ref Vector3Wide accumulatedImpulse, ref BodyInertias inertiaA, ref BodyInertias inertiaB)
        {
            ComputeCorrectiveImpulse(ref velocityA, ref velocityB, ref offsetA, ref offsetB, ref biasVelocity, ref effectiveMass, ref softnessImpulseScale, ref accumulatedImpulse, out var correctiveImpulse);
            // 此功能没有最大脉冲限制,因此不需要夹持。
            Vector3Wide.Add(accumulatedImpulse, correctiveImpulse, out accumulatedImpulse);

            ApplyImpulse(ref velocityA, ref velocityB, ref offsetA, ref offsetB, ref inertiaA, ref inertiaB, ref correctiveImpulse);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref Vector3Wide offsetA, ref Vector3Wide offsetB,
            ref Vector3Wide biasVelocity, ref Symmetric3x3Wide effectiveMass, ref Vector<float> softnessImpulseScale, ref Vector<float> maximumImpulse, ref Vector3Wide accumulatedImpulse, ref BodyInertias inertiaA, ref BodyInertias inertiaB)
        {
            ComputeCorrectiveImpulse(ref velocityA, ref velocityB, ref offsetA, ref offsetB, ref biasVelocity, ref effectiveMass, ref softnessImpulseScale, ref accumulatedImpulse, out var correctiveImpulse);
            // 此函数有最大脉冲限制。
            ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulse, ref correctiveImpulse);
            Vector3Wide.Add(accumulatedImpulse, correctiveImpulse, out accumulatedImpulse);

            ApplyImpulse(ref velocityA, ref velocityB, ref offsetA, ref offsetB, ref inertiaA, ref inertiaB, ref correctiveImpulse);
        }

    }
}
